%author: wxj233
%time: 2023.10.25 12:00
%function: 主要用于完成特征点仿真以及轨迹模拟，整个场景在二维平面上进行

clc;
clear;
clear Function;
close all;


% 初始化仿真器
% dt
simulater = Simulater(1);

% 产生第一个目标特征点群
% % len, width, probabilitys, varargin{seed}
% cluster1 = simulater.generateFuturePoints(10, 2.2, [0.8, 0.9, 0.7, 0.8], 1);
% % 根据特征点群，产生第一条轨迹
% % cluster, point0, v, a, startTime, endTime, id, sd, varargin
% T1 = simulater.generateTrack(cluster1, [10, 0], [0, 1], [0.01, -0.005], 0, 100, 1, 0.1, 10);  % [t, x, y, id; ...]

% 产生第二个目标特征点群
% len, width, probabilitys, varargin{seed}
cluster2 = simulater.generateFuturePoints(5, 2.2, [0.9, 0.8, 0.7], 3);
% 根据特征点群，产生第二条轨迹
% cluster, point0, v, a, startTime, endTime, id, sd, varargin
T2 = simulater.generateTrack(cluster2, [90, 0], [0, 1], [-0.01, -0.005], 0, 100, 2, 0.1, 10);  % [t, x, y, id; ...]

% 引入噪声, 根据区域大小和噪声密度来算噪声数量，且数量服从泊松分布, 在区域内服从均匀分布，区域自己根据轨迹范围设定
% density, area
% noise = simulater.generateNoise(0.001, [0, 100, -20, 120], 1); % [t, x, y, 0; ...]

% vMax, r, q, p, vAssDoor, inDoor, fplifeMax, tlifeMax
mfpTracer = MFPTracer(10, 0.2, 0.01, 1, 0.95, 0.45, 20, 3);
[frame, isNextFrame]= simulater.getNextFrame();
frames = [];
while isNextFrame
    frame = mfpTracer.preproccess(frame, 20, 2, 1, 1);  % 预处理
    frames = [frames; frame];
    figure(1);
    gscatter(frames(:,2), frames(:,3), frames(:,4));hold on;
    disp("当前第："+num2str(simulater.timeIndex)+"帧");  % 还未被追踪
    for track = mfpTracer.Tracks
        for fp = track.fps
            plot(fp.points(:,2), fp.points(:,4));hold on;
            te = "T"+num2str(fp.track.id)+" f:"+num2str(fp.id);
            text(fp.points(end,2), fp.points(end,4), te);
        end
    end
    hold off;
    axis([0 100 -20 120]);
    title("轨迹信息"+num2str(simulater.timeIndex));
    xlabel("x(m)");
    ylabel("y(m)");
    legend([]);
    grid on;
    if simulater.timeIndex == 12
        disp("pause");
    end
    
    mfpTracer.tracer(frame);
    [frame, isNextFrame]= simulater.getNextFrame();
end


